from matplotlib import pyplot as plt
from matplotlib import animation
import numpy as np
import pandas as pd
import seaborn as sns
import random
import matplotlib.path as mpath  
import matplotlib.patches as patches  
import matplotlib.transforms as transforms
from icon import icon

import sys 
sys.path.insert(0, sys.path[0]+"/../")

from utils.utils import *
from algos import *



# 防止中文乱码
plt.rcParams['font.sans-serif']=['SimHei']
plt.rcParams['axes.unicode_minus']=False


if __name__ == '__main__':
    print("indeed")
    # 数据处理
    data = pd.read_csv(r"log/log_99/swarm_log.csv")
    # print(type(data.columns()))
    # print(data.shape[1])
    num = 3#int((data.shape[1] - 1) / 12) #12个参数
    wingspan = 1.363
    baseline_x = wingspan   # np.pi / 4 *
    baseline_y = 2 * wingspan

    timestamp_data = data[["timestamp"]]
    # print(timestamp.head())
    # print(timestamp.iloc[0,0])
    timestamp_data = timestamp_data.sub(timestamp_data.iloc[0,0]).div(1000)
    # print(timestamp_data.head())

    data_err_x = data[["delta_x" + str(i+1) for i in range(num)]].rolling(window=45).mean() # 纵向位置差
    data_err_y = data[["delta_y" + str(i+1) for i in range(num)]] # 横向位置差
    data_z = data[["D" + str(i+1) for i in range(num)]]
    
    data_err_z = data_z.sub(data["D1"],axis=0)
    data_err_z = data_err_z.rolling(window=15).mean()
    # TODO：数据处理，使炸机情况仍可通过可视化
    data_err_x.fillna(0)
    data_err_y.fillna(0)
    
    data_mean_err_x = data_err_x.replace(-1, 0)
    data_mean_err_y = data_err_y.replace(-1, 0)

    data_mean_err_x = data_mean_err_x.abs().mean(axis=1)
    data_mean_err_y = data_mean_err_y.abs().mean(axis=1)

    data_rmean_err_x = data_mean_err_x.div(baseline_x).mul(100)
    data_rmean_err_y = data_mean_err_y.div(baseline_y).mul(100)

    data_N = data[["N" + str(i+1) for i in range(num)]]
    data_E = data[["E" + str(i+1) for i in range(num)]]
    data_yaw = data[["yaw" + str(i+1) for i in range(num)]]
    # print(type(data_N["N1"]))
    data_relative_N = data_N.sub(data_N["N1"], axis=0)
    # print(data_relative_N.head())
    data_relative_E = data_E.sub(data_E["E1"], axis=0)
    
    # print(data_relative_E.head())
    
    # 配置色彩文件
    color = sns.husl_palette(len(data_err_x.columns),h=15/360, l=.65, s=1).as_hex() 
    # random.shuffle(color)
    colors_x = dict(zip(data_err_x.columns.tolist(),color))
    colors_y = dict(zip(data_err_y.columns.tolist(),color))
    # patch = patches.PathPatch(icon, facecolor=colors["id1"], lw=0.5)

    # plt.ion()

    [[1,0.,0.,0.,30.],
          [2,-200.,-200.,0.,30.],
          [3,-200.,200.,0.,30.],
          [4,-300.,-200.,0.,30.],
          [5,-300.,200.,0.,30.],
          [6,-400.,-200.,0.,30.],
          [7,-400.,200.,0.,30.],
          [8,-500.,-200.,0.,30.],
          [9,-500.,200.,0.,30.],]


    plt.figure(figsize=(10, 8))
    ax1 = plt.subplot(1, 1, 1)
    ax1.grid(True)
    # ax2 = plt.subplot(3, 1, 2)
    # ax3 = plt.subplot(3, 1, 3)
    
    timestamp=[]
    # invalid_col_idx=[]
    # id_alive = [ i+1 for i in range(num)]

    start = 1970 # 时间轴起始
    box = 170   # 可视化区间
    
    # for j in range(3):
    #     ax1.plot(timestamp_data.iloc[start:start+box,0].sub(395) ,data_err_x.iloc[start:start+box,j], color=colors_x[data_err_x.columns.tolist()[j]], label='delta_x'+str(j+1))
    #     ax2.plot(timestamp_data.iloc[start:start+box,0].sub(395) ,data_err_y.iloc[start:start+box,j], color=colors_y[data_err_y.columns.tolist()[j]], label='delta_y'+str(j+1))
    #     ax3.plot(timestamp_data.iloc[start:start+box,0].sub(395) ,data_err_z.iloc[start:start+box,j], color=colors_y[data_err_y.columns.tolist()[j]], label='delta_z'+str(j+1))
        
        # ax1.legend(loc='best')
        # ax1.set_title("VerticalError")
        # ax1.set_ylabel("ErrValue/m")
    plt.show()
    
